import time
import vehicle

# 颜色编码映射（根据你的Rust常量定义）
COLOR_CODE = {
    'red': 0x01,
    'green': 0x02,
    'blue': 0x03,
    'yellow': 0x04,
    'purple': 0x05,
    'indigo': 0x06,
    'white': 0x07
}

def led_demo(car, interval=1):
    for i in range(4):
        car.start_motor(i)
    try:
        while True:
            # 循环点亮各色LED
            for color_name, code in COLOR_CODE.items():
                print(f"点亮 {color_name.upper()}")
                car.led_light_on(code)  # 打开指定颜色
                time.sleep(interval)
                car.led_light_off()     # 关闭
                time.sleep(0.5)
                
    except KeyboardInterrupt:
        print("\n程序终止")
        car.led_light_off()
        for i in range(4):
            car.stop_motor(i)
    except Exception as e:
        print(f"发生错误: {e}")
        car.led_light_off()
        for i in range(4):
            car.stop_motor(i)

if __name__ == "__main__":
    try:
        print("初始化车辆控制...")
        car = vehicle.Vehicle()
        print("开始LED演示 (按 Ctrl+C 停止)")
        led_demo(car, interval=2)
        
    except Exception as e:
        print(f"初始化失败: {str(e)}")